#ifndef HITSZ_DIP_PRACTICUM_DETECTOR_H
#define HITSZ_DIP_PRACTICUM_DETECTOR_H

#include <vector>
#include <opencv2/opencv.hpp>
#include "param.h"
#include <Eigen/Dense>
namespace dip
{ //这个不是泛用的，所以使用dip
struct HSV_Threshold
{
    int h_min;
    int h_max;
    int s_min;
    int s_max;
    int v_min;
    int v_max;
};
class ColumnDetector
    {
        public:
            ColumnDetector()
            {
                this->init_threshold();
            }
            ~ColumnDetector(){};

            /**
             * @brief 重新设置阈值
             */ 
            void reset()
            {
                hsv_thre_list.clear();
                hsv_thre_list.resize(2);//和ex4_color一样的长度和顺序
                hsv_thre_list[0].h_min = Param::LC_H_MIN;
                hsv_thre_list[0].h_max = Param::LC_H_MAX;
                hsv_thre_list[0].s_min = Param::LC_S_MIN;
                hsv_thre_list[0].s_max = Param::LC_S_MAX;
                hsv_thre_list[0].v_min = Param::LC_V_MIN;
                hsv_thre_list[0].v_max = Param::LC_V_MAX;

                hsv_thre_list[1].h_min = Param::RC_H_MIN;
                hsv_thre_list[1].h_max = Param::RC_H_MAX;
                hsv_thre_list[1].s_min = Param::RC_S_MIN;
                hsv_thre_list[1].s_max = Param::RC_S_MAX;
                hsv_thre_list[1].v_min = Param::RC_V_MIN;
                hsv_thre_list[1].v_max = Param::RC_V_MAX;
            }

            /**
             * @brief 对颜色进行彩色分割，提取最大轮廓并获取中心对应3d坐标
             * @param img,传入图像,应该为BGR格式
             * @param depth,和彩色图配准的深度图像
             * @param output,输出二值化图像 vector,主要是用于debug
             * @param img_points,输出图像坐标
             * @param points,输出点位置，为L和R，单位m
             * @return 是否检测到两个圆柱并获取到其坐标
             */ 
            bool detect_column(const cv::Mat &img,const cv::Mat &depth,std::vector<cv::Mat> &output,std::vector<cv::Point>& img_points,std::vector<Eigen::Vector3f> &points)
            {
                output.clear();
                points.clear();
                // points.resize(2);//
                img_points.clear();
                // img_points.resize(2);
                cv::Mat hsv_img; 
                cv::cvtColor(img,hsv_img,cv::COLOR_BGR2HSV);//颜色通道转换

                //hsv segmant
                this->segmant_color(hsv_img,output);

                //findcounter
                //从各个二值化图像中各提取一个rect
                std::vector<cv::Rect> vBoundingBox;
                for (int k=0;k<output.size();k++)
                {
                    std::vector<std::vector<cv::Point>> vContours; //轮廓
                    cv::findContours(output[k],vContours,cv::RETR_EXTERNAL,cv::CHAIN_APPROX_NONE);
                    //多边形拟合
                    std::vector<std::vector<cv::Point>> vRects; //为矩形的轮廓
                    
                    for (int i=0;i<vContours.size();i++)
                    { //判断轮廓是否为四边形
                        std::vector<cv::Point> approxPloy;
                        float epsilon = cv::arcLength(vContours[i], true)*0.02;
                        cv::approxPolyDP(vContours[i], approxPloy, epsilon, true);
                        if (fabs(cv::contourArea(approxPloy)) > 600)
                        {
                        // if (approxPloy.size() == 4 &&fabs(cv::contourArea(approxPloy)) > 1000 &&cv::isContourConvex(approxPloy))
                        // { //计算余弦,满足条件为矩形
                            // double maxCosine = 0;

                            // for (int j = 2; j < 5; j++)
                            // {
                            //     // 求轮廓边缘之间角度的最大余弦
                            //     double cosine = fabs(calc_angle(approxPloy[j % 4],approxPloy[j - 2],approxPloy[j - 1]));
                            //     maxCosine = MAX(maxCosine, cosine);
                            // }

                            // if (maxCosine < 0.3)
                            // {
                            //     vRects.push_back(approxPloy);
                            // }
                            vRects.push_back(approxPloy);
                        }
                    }

                    //提取面积最大的矩形
                    if (vRects.size()==0)
                    { //如果没有合适的矩形
                        // points[k] = Eigen::Vector3f::Zero();
                        // img_points[k] = cv::Point(0,0);
                        continue;
                    }
                    //如果找到了矩形
                    
                    double max_area = 0;
                    int index = 0;
                    for (int j=0;j<vRects.size();j++)
                    { //find max rect
                        double area = cv::contourArea(vRects[j],false);
                        if (area > max_area)
                        {
                            max_area = area;
                            index = j;
                        }
                    }
                    auto objRect = vRects[index];

                    //提取面积最大的矩形的BoundingRect作为roi
                    cv::Rect boundRect = cv::boundingRect(objRect);
                    vBoundingBox.push_back(boundRect);
                }

                
                if (vBoundingBox.size() <= 1)
                { //如果没有提取到圆柱或只有一个，则认为没有检测到
                    return false;
                }

                for (int i=0;i<vBoundingBox.size();i++)
                { //计算中心，并得到世界坐标
                    cv::Point center = cv::Point(vBoundingBox[i].x+vBoundingBox[i].width/2,vBoundingBox[i].y+vBoundingBox[i].height/2);
                    img_points.push_back(center);
                    //不考虑畸变，由于已经将深度图配准到彩色图下直接计算
                    // float z = static_cast<float>(depth.at<uint16_t>(center.y,center.x));//at先y后x
                    // std::cout<<"img point x,y:"<<center.x<<","<<center.y<<",z:"<<z<<std::endl;
                    Eigen::Vector3f p3d;
                    // //rnm,opencv at(y,x)
                    // float py = ((center.x - Param::KK.at<double>(0,2))*z/Param::KK.at<double>(0,0))/1000.0f;
                    // float pz = -((center.y - Param::KK.at<double>(1,2))*z/Param::KK.at<double>(1,1))/1000.0f;
                    // float px = z/1000.0f;
                    // p3d<<px,py,pz;
                    bool get_coords = this->calc_3dpoints(depth,center,p3d);
                    if (!get_coords){return false;}
                    points.push_back(p3d);
                }

                //判断颜色
                // std::vector<int> pixel_counts;
                // EX4_COLOR color = this->assert_roi_color(hsv_img,vBoundingBox[index],pixel_counts);
                // output.push_back(hsv_img.clone());
                return true;
            }
            
            /**
             * @brief hsv图像提取各个颜色的二值化图像
             * @param src ，hsv图像
             * @param output，输出的mat vector
             */ 
            void segmant_color(const cv::Mat &src,std::vector<cv::Mat> &output)
            {
                int n = this->hsv_thre_list.size();
                output.clear();
                cv::Mat thre_img;
                for (int i=0;i<n;i++)
                { //对每个颜色进行分割
                    HSV_Threshold hsv_thre = this->hsv_thre_list[i];
                    cv::inRange(src,cv::Scalar(hsv_thre.h_min,hsv_thre.s_min,hsv_thre.v_min), 
                        cv::Scalar(hsv_thre.h_max,hsv_thre.s_max,hsv_thre.v_max),thre_img); //Threshold the image 
                    cv::Mat kernel = cv::getStructuringElement(cv::MORPH_RECT,cv::Size(5,5));
                    cv::Mat k2 = cv::getStructuringElement(cv::MORPH_RECT,cv::Size(7,7));
                    cv::morphologyEx(thre_img,thre_img,cv::MORPH_OPEN,kernel);
                    cv::morphologyEx(thre_img,thre_img,cv::MORPH_CLOSE,k2);
                    output.push_back(thre_img.clone());
                }
            }
        private:
            void init_threshold(void)
            {
                hsv_thre_list.clear();
                hsv_thre_list.resize(2);//和ex4_color一样的长度和顺序
                hsv_thre_list[0].h_min = Param::LC_H_MIN;
                hsv_thre_list[0].h_max = Param::LC_H_MAX;
                hsv_thre_list[0].s_min = Param::LC_S_MIN;
                hsv_thre_list[0].s_max = Param::LC_S_MAX;
                hsv_thre_list[0].v_min = Param::LC_V_MIN;
                hsv_thre_list[0].v_max = Param::LC_V_MAX;
                hsv_thre_list[1].h_min = Param::RC_H_MIN;
                hsv_thre_list[1].h_max = Param::RC_H_MAX;
                hsv_thre_list[1].s_min = Param::RC_S_MIN;
                hsv_thre_list[1].s_max = Param::RC_S_MAX;
                hsv_thre_list[1].v_min = Param::RC_V_MIN;
                hsv_thre_list[1].v_max = Param::RC_V_MAX;
            }

            /**
             * @brief 根据中心点计算对应xyz 3d坐标
             * @param 
             * @return 如果深度值都为0，那么返回false
             */ 
            bool calc_3dpoints(const cv::Mat &depth,const cv::Point &center,Eigen::Vector3f &p3d)
            {
                auto clamp =[](int &x1,int &y1,int &x2,int &y2,const int &cols,const int &rows)
                {
                    x1 = x1<0?0:x1;
                    y1 = y1<0?0:y1;
                    x2 = x2>cols?cols:x2;
                    y2 = y2>rows?rows:y2;
                };
                //cv::Rect rect(x1,y1,x2 - x1,y2 -y1);//roi区域
                int width = 4;
                int height = 4;
                int x1 = center.x - width/2;
                int x2 = center.x + width/2;
                int y1 = center.y - height/2;
                int y2 = center.y + height/2;
                clamp(x1,y1,x2,y2,depth.cols,depth.rows);//限幅

                // std::vector<float> x_points;
                // std::vector<float> y_points;
                // std::vector<float> z_points;
                int cnt =0;
                float x_point=0;
                float y_point=0;
                float z_point=0;
                //遍历邻域计算结果
                for (int i=x1;i<=x2;i++)
                {
                    for(int j=y1;j<=y2;j++)
                    {
                        float z = static_cast<float>(depth.at<uint16_t>(j,i));//at先y后x
                        if (z<1e-5)
                        { //无深度跳过
                            continue;
                        }
                        else
                        {
                            float py = ((center.x - Param::KK.at<double>(0,2))*z/Param::KK.at<double>(0,0))/1000.0f;
                            float pz = -((center.y - Param::KK.at<double>(1,2))*z/Param::KK.at<double>(1,1))/1000.0f;
                            float px = z/1000.0f;
                            // x_points.push_back(px);
                            // y_points.push_back(py);
                            // z_points.push_back(pz);
                            cnt++;
                            x_point +=px;
                            y_point +=py;
                            z_point +=pz;
                        }
                    }
                }

                if (cnt==0)
                { //如果全零无深度
                    return false;
                }
                else
                {
                    x_point = x_point/(cnt*1.0f);
                    y_point = y_point/(cnt*1.0f);
                    z_point = z_point/(cnt*1.0f);
                    // std::cout<<"eigen fxxking"<<std::endl;
                    p3d<<x_point,y_point,z_point;
                    return true;
                }
            }

        public:
            std::vector<HSV_Threshold> hsv_thre_list; //HSV阈值

        private:

    };
}

#endif 